using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  public class MotionModel2D : MotionModel
  {
    public override bool StatesAreNearBy(WorldState oldState, WorldState newState)
    {
      double movedDistanceX = (newState as WorldState2D).Vector.x - (oldState as WorldState2D).Vector.x;
      double movedDistanceY = (newState as WorldState2D).Vector.y - (oldState as WorldState2D).Vector.y;

      int movedStepsX = (int)(movedDistanceX / Globals.motionRasterWidth);
      int movedStepsY = (int)(movedDistanceY / Globals.motionRasterWidth);

      if (Math.Abs(movedStepsX) > 2 || Math.Abs(movedStepsY) > 2) return false;
      else return true;
    }
    //2DO!!!
    public override double GetStateTransitionProbability(WorldState oldState, WorldState newState, ActionDescription action)
    {
      if (action is DiscreteAction2D)
        return GetStateTransitionProbabilityForDiscreteAction(oldState, newState, action as DiscreteAction2D);
      else if (action is ContinuousAction2D)
        return GetStateTransitionProbabilityForContinuousAction(oldState, newState, action as ContinuousAction2D);
      else return 0;
    }

    private static double GetStateTransitionProbabilityForContinuousAction(WorldState oldState, WorldState newState, ContinuousAction2D action)
    {
      //ATTENTION!!! CHANGES IN THIS METHOD MIGHT REQUIRE CHANGES IN METHOD DoAction !!!
      Vector2 oldPosition = new Vector2((oldState as WorldState2D).Vector.x, (oldState as WorldState2D).Vector.y);
      Vector2 newPosition = new Vector2((newState as WorldState2D).Vector.x, (newState as WorldState2D).Vector.y);
      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.gaussianNoSystematic)
      {
        return
          Globals.Gaussian((oldPosition - newPosition).Abs() - action.action.Abs(), 0.050, 0);
      }
      else if (Globals.selfLocatorProperties.MotionNoise == NoiseType.noNoise)
      {
        if (Math.Abs((newPosition.x - action.action.x) - oldPosition.x) < 10 && Math.Abs((newPosition.y - action.action.y) - oldPosition.y) < 10)
        {
          return 1;
        }
        else return 0;
      }
      else return 0;
    }

    private static double GetStateTransitionProbabilityForDiscreteAction(WorldState oldState, WorldState newState, DiscreteAction2D action)
    {
      //ATTENTION!!! CHANGES IN THIS METHOD MIGHT REQUIRE CHANGES IN METHOD DoAction !!!
      double movedDistanceX = (newState as WorldState2D).Vector.x - (oldState as WorldState2D).Vector.x;
      double movedDistanceY = (newState as WorldState2D).Vector.y - (oldState as WorldState2D).Vector.y;

      int movedStepsX = (int)(movedDistanceX / Globals.motionRasterWidth);
      int movedStepsY = (int)(movedDistanceY / Globals.motionRasterWidth);

      int signX = 0;
      int signY = 0;
      if (action.typeOfAction == DiscreteAction2D.TypeOfAction.moveLeft) signX = -1;
      if (action.typeOfAction == DiscreteAction2D.TypeOfAction.moveRight) signX = +1;
      if (action.typeOfAction == DiscreteAction2D.TypeOfAction.moveDown) signY = -1;
      if (action.typeOfAction == DiscreteAction2D.TypeOfAction.moveUp) signY = +1;

      double motionCommandX = signX * Globals.motionRasterWidth;
      double motionCommandY = signY * Globals.motionRasterWidth;

      double probability = 0;
      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.gaussianNoSystematic)
      {
        if (movedStepsX == signX && movedStepsY == signY)
          probability = 0.8;
        else if ((Math.Abs(signX) == 1 || Math.Abs(signY) == 1) && movedDistanceX == 0 && movedDistanceY == 0)
          probability = 0.1;
        else if ((Math.Abs(signX) == 1 || Math.Abs(signY) == 1) && movedDistanceX == 2 && movedDistanceY == 2)
          probability = 0.1;
        else
          probability = 0;
      }
      else if (Globals.selfLocatorProperties.MotionNoise == NoiseType.equal)
      {
      }
      else if (Globals.selfLocatorProperties.MotionNoise == NoiseType.noNoise)
      {
        if (movedStepsX == signX && movedStepsY == signY) probability = 1;
        else probability = 0;
      }
      return probability;
    }
  }
}
